clear all;clc;
X=[49.994859	50	50	49.999217	49.968967	60
5	5	5	1.3476638	4.785659	5
5.915643	5.905345	7.031047	1.3476825	6.491026	7.06916
49.994867	50	48.134183	49.999235	48.393942	58.1241
18.925715	18.819312	16.976687	11.383058	16.444782	16.8655
0	0	12.952139	4.4426855	11.988091	13.0576
14.472475	14.373772	12.197494	10.1944353	12.046587	11.8936
-12.494409	-12.444295	-15.998203	-3.6925458	-14.774897	-16.0037
0.467287	0.463633	0.0428247	6.2157809	0.038678	6.28318
]
for j=1:size(X,2)
x=(X(:,j))% 
C1=[0;0];
C2=[1.9098 ;5.8779];
C3=[6.9098; 9.5106];
C4=[13.09; 9.5106];
C5=[18.09 ;5.8779];
C6=[20 ;0.0];
% Quy dao cho truoc
CD=[C1,C2,C3,C4,C5,C6];
% ==========================
Cdx=CD(1,:); Cdy=CD(2,:);
% ==========================
r1=x(1); r2=x(2);
r3=x(3); r4=x(4);
rcx=x(5); rcy=x(6);
f=0;
Teta2=[pi/6;pi/3;pi/2;2*pi/3;5*pi/6;pi];
for i=1:length(Cdx)
    % teta 3
    teta2=Teta2(i);
    K1=r1/r2;
    K4=r1/r3;
    K5=(r4^2-r1^2-r2^2-r3^2)/(2*r3*r2);
    D=cos(teta2)-K1+K4*cos(teta2)+K5;
    E=-2*sin(teta2);
    F=K1+(K4-1)*cos(teta2)+K5;
    ATAN3=(-E-sqrt(E^2-4*D*F))/(2*D);
    teta3=real(2*atan(ATAN3));
    Crx=r2*cos(teta2)+rcx*cos(teta3)-rcy*sin(teta3);
    Cry=r2*sin(teta2)+rcx*sin(teta3)+rcy*cos(teta3);
    C(:,i)=[cos(x(9)),-sin(x(9));sin(x(9)),cos(x(9))]*[Crx;Cry]+[x(7);x(8)];
    f=f+sum((C(:,i)-[Cdx(i);Cdy(i)]).^2);
end
x=[x;f];
DATA(:,j)=x;
end